#ifndef _TASK_MANAGER_H__
#define _TASK_MANAGER_H__

// 2023.12.15 modify by haibo
#include <iostream>
#include <string>
#include <map>
#include <vector>
#include <memory>
#include <chrono>
#include <thread>
#include <mutex>
#include <fstream>
#include <opencv2/opencv.hpp>
#include <nlohmann/json.hpp>
#include "mqtt/async_client.h"
#include "yaml-cpp/yaml.h"

#include <rclcpp/rclcpp.hpp>
#include <rclcpp/executors/multi_threaded_executor.hpp>
#include <geometry_msgs/msg/point.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <nav_msgs/msg/occupancy_grid.hpp>
#include "agv_msgs/msg/map_node.hpp"
#include "agv_msgs/msg/state.hpp"
#include "agv_msgs/msg/action.hpp"
#include "agv_msgs/msg/action_param.hpp"
#include "agv_msgs/msg/agv_mode.hpp"
#include "agv_msgs/msg/bat_info.hpp"
#include "agv_msgs/msg/point_cloud_pub_switch.hpp"
#include "agv_msgs/msg/agv_event_status.hpp"
#include "agv_msgs/msg/periph_status.hpp"
#include "agv_msgs/msg/agv_event.hpp"
#include "agv_msgs/msg/agv_auto_status.hpp"
#include "agv_msgs/msg/diagnosis.hpp"
#include "agv_msgs/msg/connection_monitor.hpp"
#include "agv_srvs/srv/order.hpp"
#include "agv_srvs/srv/instant_actions.hpp"


#include "json_serialization.h"

using json = nlohmann::json;
using namespace cv;
using ordered_json = nlohmann::ordered_json; // 显式使用有序JSON类型
// Constants
const std::string ORDER_TOPIC = "order";
const std::string INSTANT_ACTION = "instantActions";
const std::string STATE_TOPIC = "state";
const std::string CONNECTION_TOPIC = "connection";
const std::string LOCALIZATION_TOPIC = "localization";
const std::string QR_LOCALIZATION_REQUEST_TOPIC = "qrLocalizationRequest";
const std::string QR_LOCALIZATION_RESPONSE_TOPIC = "qrLocalizationResponse";
const std::string POINT_CLOUD_SWITCH = "pointCloudSwitch";
const std::string SAVE_MAP_TOPIC = "saveMap";
const std::string SAVE_MAP_RESPONSE_TOPIC = "saveMapResponse";
const std::string NAV_MODE_TOPIC = "navMode";
const std::string NAV_MODE_RESPONSE_TOPIC = "navModeResponse";
const std::string MAP_TOPIC = "map";
const std::string POINT_CLOUD_TOPIC = "pointCloud";
const std::string MAP_MODIFY_TOPIC = "mapModify";
const std::string MAP_MODIFY_RESPONSE_TOPIC = "mapModifyResponse";
const std::string MAP_MERGE_TOPIC = "mapMerge";
const std::string MAP_MERGE_RESPONSE_TOPIC = "mapMergeResponse";
const std::string UPDATEQR_TOPIC = "updateQR";
const std::string UPDATEQR_RESPONSE_TOPIC = "updateQrResponse";
const std::string ORDER_ID_TOPIC = "orderIdTopic";

// Operating modes
const std::string AUTOMATIC_OP_MODE = "AUTOMATIC";
const std::string SEMIAUTOMATIC_OP_MODE = "SEMIAUTOMATIC";
const std::string MANUAL_OP_MODE = "MANUAL";
const std::string SERVICE_OP_MODE = "SERVICE";
const std::string TEACHIN_OP_MODE = "TEACHIN";

// Action status
const std::string ACTION_WAITING = "WAITING";
const std::string ACTION_INITIALIZING = "INITIALIZING";
const std::string ACTION_RUNNING = "RUNNING";
const std::string ACTION_PAUSED = "PAUSED";
const std::string ACTION_FINISHED = "FINISHED";
const std::string ACTION_FAILED = "FAILED";


class TaskManager: public mqtt::callback  {
public:
    TaskManager(const std::shared_ptr<rclcpp::Node> node);
    void run();

private:
    bool initParameters();
    bool loadConfig(const std::string& config_path);
    void loadInitialState();
    void getParam();
    void initializeROSComponents();
    void initMQTT();
    void connectToMQTTBroker();

    void initializeTimers();
    // Callback methods
    void stateCB(agv_msgs::msg::State::ConstSharedPtr msg);
    void agvModeCB(agv_msgs::msg::AGVMode::ConstSharedPtr msg);
    void batteryInfoCB(agv_msgs::msg::BatInfo::ConstSharedPtr msg);
    void scanCB(sensor_msgs::msg::PointCloud2::ConstSharedPtr msg);
    void eventCB(agv_msgs::msg::AGVEventStatus::ConstSharedPtr msg);
    void periphStatusCB(agv_msgs::msg::PeriphStatus::ConstSharedPtr msg);
    void agvEventCB(agv_msgs::msg::AGVEvent::ConstSharedPtr msg);
    void agvStatusCB(agv_msgs::msg::AgvAutoStatus::ConstSharedPtr msg);
    void agvDiagnosisCB(agv_msgs::msg::Diagnosis::ConstSharedPtr msg);
    void mapCB(nav_msgs::msg::OccupancyGrid::ConstSharedPtr msg);
    void switchCB(agv_msgs::msg::PointCloudPubSwitch::ConstSharedPtr msg);

    // Timer callback methods
    void publishState();
    void publishConnectionState();
    void publishOrderId();
    void stateJSON();
    void publishPointCloud();
    void publishMap();
    void publishError();

    // Helper methods
    std::string getEnvVar(const std::string& key);

    // MQTT callback implementation
    void message_arrived(mqtt::const_message_ptr msg) override;
    
    void connection_lost(const std::string& cause) override;

    void connected(const std::string& tok) override;
    void subTopic();
    // MQTT message handlers
    void orderHandle(const json& order);

    void changeOrder(const json& order);

    void instantActionHandle(const json& instant_order);

    void connectionHandle(const json& connection_msg);

    void localizationHandle(const json& localization_msg);

    void qrLocalizationHandle(const json& qr_localization_msg); 

    void pcSwitchHandle(const json& pc_switch_msg);

    void saveMapHandle(const json& save_map_msg);

    void navModeHandle(const json& nav_mode_msg);

    void mapModifyHandle(const json& msg);

    void updateQRHandle(const json& msg);

    void mapMergeHandle(const json& msg);

    // Member variables
    std::unique_ptr<mqtt::async_client> mqtt_client_;
    std::shared_ptr<rclcpp::Node> node_;
    rclcpp::executors::MultiThreadedExecutor::SharedPtr main_executor_;

    rclcpp::Client<agv_srvs::srv::Order>::SharedPtr order_client_;
    rclcpp::Client<agv_srvs::srv::InstantActions>::SharedPtr instant_actions_client_;

    // ROS subscribers
    rclcpp::Subscription<agv_msgs::msg::State>::SharedPtr state_sub_;
    rclcpp::Subscription<agv_msgs::msg::AGVMode>::SharedPtr agv_mode_sub_;
    rclcpp::Subscription<agv_msgs::msg::BatInfo>::SharedPtr battery_info_sub_;
    rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr scan_sub_;
    rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr map_sub_;
    rclcpp::Subscription<agv_msgs::msg::PointCloudPubSwitch>::SharedPtr switch_sub_;
    rclcpp::Subscription<agv_msgs::msg::AGVEventStatus>::SharedPtr event_status_sub_;
    rclcpp::Subscription<agv_msgs::msg::PeriphStatus>::SharedPtr periph_status_sub_;
    rclcpp::Subscription<agv_msgs::msg::AGVEvent>::SharedPtr agv_event_sub_;
    rclcpp::Subscription<agv_msgs::msg::AgvAutoStatus>::SharedPtr auto_status_sub_;
    rclcpp::Subscription<agv_msgs::msg::Diagnosis>::SharedPtr diagnosis_sub_;
    
    // ROS publishers
    rclcpp::Publisher<agv_msgs::msg::ConnectionMonitor>::SharedPtr connection_pub_;
    rclcpp::Publisher<agv_msgs::msg::PointCloudPubSwitch>::SharedPtr pc_switch_pub_;
    rclcpp::Publisher<agv_msgs::msg::AGVEvent>::SharedPtr error_pub_;
    
    // ROS timers
    rclcpp::TimerBase::SharedPtr publish_state_timer_;
    rclcpp::TimerBase::SharedPtr publish_order_id_timer_;
    rclcpp::TimerBase::SharedPtr state_json_timer_;
    rclcpp::TimerBase::SharedPtr publish_point_cloud_timer_;
    rclcpp::TimerBase::SharedPtr publish_map_timer_;
    rclcpp::TimerBase::SharedPtr publish_error_timer_;
    rclcpp::TimerBase::SharedPtr publish_connection_state_timer_;
    
    // Configuration
    json config_;
    std::string agv_num_;
    std::string broker_ip_;
    int broker_port_;
    int netInterval_;
    std::string schemas_path_;
    std::string msg_path_;
    
    // State variables
    State state_;
    ordered_json json_object_;
    json orderIdMsg_;
    json point_msg_;
    json map_msg_;
    json heart_bit_state_;
    json send_info_;
    json qrLocation_;
    json map_modify_ret_json_;
    
    // Other variables
    bool pc_pub_switch_;
    bool map_pub_switch_;
    bool broker_disconnect_;
    bool master_disconnect_;
    int navigation_mode_;
    std::string description_;
    std::vector<json> point_list_;
    int seq_ = 0;
    int header_id_ = 0;
    
    // Timing variables
    std::chrono::system_clock::time_point time_monitor_old_;
    std::chrono::system_clock::time_point time_monitor_new_;
    std::chrono::system_clock::time_point t_old_;
    std::chrono::system_clock::time_point t_now_;
    
    // Mutexes
    std::mutex state_mutex_;
    std::mutex order_mutex_;
    std::mutex point_cloud_mutex_;
    std::mutex map_mutex_;
};

#endif
